home *** CD-ROM | disk | FTP | other *** search
/ Cream of the Crop 26 / Cream of the Crop 26.iso / os2 / octa209s.zip / octave-2.09 / scripts / control / dlqr.m < prev    next >
Text File  |  1997-06-25  |  3KB  |  102 lines
  1. ## Copyright (C) 1996 John W. Eaton
  2. ##
  3. ## This file is part of Octave.
  4. ##
  5. ## Octave is free software; you can redistribute it and/or modify it
  6. ## under the terms of the GNU General Public License as published by
  7. ## the Free Software Foundation; either version 2, or (at your option)
  8. ## any later version.
  9. ##
  10. ## Octave is distributed in the hope that it will be useful, but
  11. ## WITHOUT ANY WARRANTY; without even the implied warranty of
  12. ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13. ## General Public License for more details.
  14. ##
  15. ## You should have received a copy of the GNU General Public License
  16. ## along with Octave; see the file COPYING.  If not, write to the Free
  17. ## Software Foundation, 59 Temple Place - Suite 330, Boston, MA
  18. ## 02111-1307, USA.
  19.  
  20. ## Usage: [k, p, e] = dlqr (A, B, Q, R {,Z})
  21. ##
  22. ## Linear quadratic regulator design for the discrete time system
  23. ##
  24. ##   x[k+1] = A x[k] + B u[k]
  25. ##
  26. ## to minimize the cost functional
  27. ##
  28. ##  J = Sum { x' Q x + u' R u }             Z omitted
  29. ##
  30. ## or
  31. ##
  32. ##  J = Sum { x' Q x + u' R u +2 x' Z u}        Z included
  33. ##
  34. ## Returns:
  35. ##
  36. ##   k = state feedback gain, (A - B K) is stable
  37. ##   p = solution of algebraic Riccati equation
  38. ##   e = closed loop poles of (A - B K)
  39.  
  40. ## Author: A. S. Hodel <scotte@eng.auburn.edu>
  41. ##         R. B. Tenison <btenison@eng.auburn.edu>
  42. ## Created: August 1993
  43. ## Adapted-By: jwe
  44.  
  45. function [k, p, e] = dlqr (a, b, q, r, zz)
  46.  
  47.   if (nargin != 4 && nargin != 5)
  48.     error ("dlqr: invalid number of arguments");
  49.   endif
  50.  
  51.   ## Check a.
  52.   if ((n = is_square (a)) == 0)
  53.     error ("dlqr: requires 1st parameter(a) to be square");
  54.   endif
  55.  
  56.   ## Check b.
  57.   [n1, m] = size (b);
  58.   if (n1 != n)
  59.     error ("dlqr: a,b not conformal");
  60.   endif
  61.  
  62.   ## Check q.
  63.  
  64.   if ((n1 = is_square (q)) == 0 || n1 != n)
  65.     error ("dlqr: q must be square and conformal with a");
  66.   endif
  67.  
  68.   ## Check r.
  69.   if((m1 = is_square(r)) == 0 || m1 != m)
  70.     error ("dlqr: r must be square and conformal with column dimension of b");
  71.   endif
  72.  
  73.   ## Check if n is there.
  74.   if (nargin == 5)
  75.     [n1, m1] = size (zz);
  76.     if (n1 != n || m1 != m)
  77.       error ("dlqr: z must be identically dimensioned with b");
  78.     endif
  79.  
  80.     ## Incorporate cross term into a and q.
  81.  
  82.     ao = a - (b/r)*zz';
  83.     qo = q - (zz/r)*zz';
  84.   else
  85.     zz = zeros (n, m);
  86.     ao = a;
  87.     qo = q;
  88.   endif
  89.  
  90.   ## Check that q, (r) are symmetric, positive (semi)definite
  91.  
  92.   if (is_symmetric (q) && is_symmetric (r) ...
  93.       && all (eig (q) >= 0) && all (eig (r) > 0))
  94.     p = dare (ao, b, qo, r);
  95.     k = (r+b'*p*b)\b'*p*ao + r\zz';
  96.     e = eig (a - b*k);
  97.   else
  98.     error ("dlqr: q (r) must be symmetric positive (semi) definite");
  99.   endif
  100.  
  101. endfunction
  102.